/******************************************************************
 * 作者：sipeiliu
 * 邮箱：sipeiliu@163.com
 * 描述：速度控制，PID算法
 * 日期：2023.4.11
 ******************************************************************/
#include "/opt/sighter/inc/optocoupler.h"
#include "/opt/sighter/inc/speed.h"
#include "/opt/sighter/inc/motor.h"

#include <stdio.h>

/*临界比例整定法初始化pid参数*/
static struct pid_parameter pid = {
    .target_speed = 500, // mm/s
    .kp = 1.08,
    .ki = 0.12,
    .kd = 0.50,
    .err = 0.0,
    .err_last = 0.0,
    .integral = 0.0,
    .throttle = 0.0,
};

/*位置式pid算法控制电机速度*/
static void pid_control(void)
{
    pid.err = pid.target_speed - global_speed;                                                   // 目标值与实际值的误差
    pid.integral = pid.integral + pid.err;                                                       // 累积误差
    pid.throttle = pid.kp * pid.err + pid.ki * pid.integral + pid.kd * (pid.err - pid.err_last); // 计算
    pid.err_last = pid.err;                                                                      // 误差更替
    // printf("%d\n", pid.throttle);
    if (pid.throttle > 0) // 给油门
    {
        motor_run(FORWARD);
        motor_run(STOP);
    }
    else if (pid.throttle < 0) // 踩刹车
    {
        motor_run(STOP);
        motor_run(FORWARD);
    }
}

/*控制速度*/
void *thread_speed(void *arg)
{
    enable_optocoupler(); // 使能光耦传感器
    while (1)
    {
        pid_control(); // 使能pid算法
    }
}